/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
// -*- coding:iso-8859-15 mode: c++ -*-
/** 
 * \file
 * \brief MaCI_CoordinateDrive Player for logged files.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * $Id: CoordinateDriveFilePlayer.cpp,v 1.2 2009-06-24 15:24:33 morsko Exp $
 *
 */

#include "CoordinateDriveFilePlayer.hpp"

CoordinateDriveFilePlayer::CoordinateDriveFilePlayer( MaCI::CoordinateDrive::CCoordinateDriveServer *aCoordinateDriveServer,
                                                      std::string aFileName)
  :PlayerModule(),
   iCoordinateDriveServer(aCoordinateDriveServer),
   iData(),
   iTimes(),
   iBegin(0,0),
   iEnd(0,0),
   iFile(NULL),
   iName("")
{
 
  
  dPrint(3,"Opening file '%s'",aFileName.c_str());
  iFile= fopen(aFileName.c_str(),"rt");

  char c[200];
  char interfaceName[64];
  //Read the four first lines..and get the interfacename...
  fgets (c, 200,iFile );

  fscanf(iFile,"%s %s %s %s",c,c,c,interfaceName);
  fgets (c, 200,iFile );
  fgets (c, 200,iFile );
 

  iCoordinateDriveServer->SetInterfaceInstanceName(std::string(interfaceName));
  iCoordinateDriveServer->Open();

  iName = "MaCI_CoordinateDrive."+std::string(interfaceName);

  ReadData();
}
//*****************************************************************************

CoordinateDriveFilePlayer::~CoordinateDriveFilePlayer()
{
  fclose(iFile);
  delete(iCoordinateDriveServer);
}
//*****************************************************************************

bool CoordinateDriveFilePlayer::ReadData()
{
  using namespace MaCI::CoordinateDrive;
  MaCI::Common::TTimestamp timestamp;
  int numberOfCoordinates;
  float x,y,a;
  int isDifferential, isPathTarget,isWaitPoint;
  int number = 1;
  int typeOfMessage;
  bool first = true;
  while(number != 0 && number != EOF){

    number = fscanf(iFile,"%u %u %d %d",
                    &(timestamp.timestamp_s),
                    &(timestamp.timestamp_us),
                    &typeOfMessage,
                    &numberOfCoordinates);
      
      
      
    if(first && number == 4){
      iBegin = timestamp.GetGimTime();
      first = false;
    }

    if(number> 0){
      CCoordinateDriveData *cData = new CCoordinateDriveData();
      cData->CreateInternalBinBag();
      if(typeOfMessage == 0){
        cData->SetCommand(TCommand(KCommandCoordinatesEvent));
        cData->SetCoordinateHeader(TCoordinateHeader(numberOfCoordinates));
      }else if(typeOfMessage == 1){
        cData->SetCommand(TCommand(KCommandArrivedToPointEvent));
      }else{
        continue;
      }
      cData->SetTimestamp(timestamp);


      for(int i = 0; i< numberOfCoordinates; i++){
        
        number = fscanf(iFile,"%f %f %f %d %d %d",
                        &x,
                        &y,
                        &a,
                        &isDifferential,
                        &isPathTarget,
                        &isWaitPoint);

        cData->SetCoordinate2D(TCoordinate2D(x,y,a,isDifferential,isPathTarget,isWaitPoint));
      }

      //fscanf(iFile,"\n");
      iData[timestamp.GetGimTime()] = cData;
      iTimes.push_back(timestamp.GetGimTime());
    }

    

  }
  if(!first){
    iEnd = timestamp.GetGimTime();
  }

  if(number == EOF){
    return true;
  }else{
    return false;
  }

}
//*****************************************************************************

bool CoordinateDriveFilePlayer::OnTime(gim::time aTime)
{
  std::map<gim::time,MaCI::CoordinateDrive::CCoordinateDriveData *>::iterator iter
    =iData.find(aTime);
  
  if(iter != iData.end()){
    const MaCI::CoordinateDrive::TCommand *cmd =(*(iter->second)).GetCommand();
    if(cmd && cmd->cmd == MaCI::CoordinateDrive::KCommandCoordinatesEvent){
      if(!iCoordinateDriveServer->SendCoordinatesEvent(*(iter->second))){
        dPrint(3,"Failed to send coordinates!");
        return false;
      }else{
        dPrint(10,"Sent coordinates event!");
      }
      return true;
    }else if(cmd && cmd->cmd == MaCI::CoordinateDrive::KCommandArrivedToPointEvent){
      if(!iCoordinateDriveServer->SendArrivedToTargetEvent(*(iter->second))){
        dPrint(3,"Failed to send arrived to target event!");
        return false;

      }else{
        dPrint(10,"Sent arrived to point event");
      }
      return true;
    }else{
      dPrint(3,"command missing or wrong command!");
      return false;
    }
  }else{
    
    return false;
  }
  
}
//*****************************************************************************

bool CoordinateDriveFilePlayer::GetTimes(std::list<gim::time> *aTimes)
{
  
  aTimes->assign(iTimes.begin(),iTimes.end());
  return true;
}

//*****************************************************************************

std::string CoordinateDriveFilePlayer::GetName(void)
{
  return iName;

}
//*****************************************************************************

void CoordinateDriveFilePlayer::GetTimeLimits(gim::time &aBegin,gim::time &aEnd)
{
  aBegin = iBegin;
  aEnd = iEnd;
  dPrint(15,"begin %s, end %s",aBegin.toString().c_str(),aEnd.toString().c_str());
}
//*****************************************************************************







